import heapq
import collections
from project_util import *
import numpy as np
import profile

Node = collections.namedtuple('Node', ['f', 'g', 'coord', 'parent'])


# class Node(object):
#     def __init__(self, f, g, coord, parent):
#         self.f = f
#         self.g = g
#         self.coord = coord
#         self.parent = parent
#
#     def __eq__(self, other):
#         if other is None or other.coord is None:
#             return False
#         return self.coord == other.coord
#
#     def __ne__(self, other):
#         return not self.__eq__(other)
#
#     def __cmp__(self, other):
#         return self.f.__cmp__(other.f)
#
#     def __hash__(self):
#         return self.coord.__hash__()
#
#     def __str__(self):
#         return self.coord.__str__()
#
#     def __repr__(self):
#         return self.__str__()


def h(t1):
    diff = subt(t1, goal)
    return abs(diff[0]) + abs(diff[1])


def succ8(s):
    return (Node(s.g + 1 + h(addt(s.coord, (x, y))), s.g + 1, addt(s.coord, (x, y)), s)
            for x in range(-1, 2) for y in range(-1, 2)
            if addt(s.coord, (x, y)) != s.coord and not soccupied(origin, addt(s.coord, (x, y))))

""" What's WRONG with this ?!
    return [Node(s.g + 1 + h(addt(s.coord, xy)), s.g + 1, addt(s.coord, xy), s)
            for xy in ((1, 0), (0, 1), (-1, 0), (0, -1))
            if addt(s.coord, xy) != s.coord and not soccupied(origin, addt(s.coord, xy))]
"""


def replace_if_exists(node, frontier):
    for n in frontier:
        if n.coord == node.coord:
            if n.f > node.f:
                frontier.remove(n)
                heapq.heapify(frontier)
                heapq.heappush(frontier, node)
                break


def contains(frontier, node):
    for n in frontier:
        if n.coord == node.coord:
            return True
    return False


def to_path(g):
    append = list.append
    path = list()
    while g.parent is not None:
        append(path, g.parent.coord)
        g = g.parent
    path.reverse()
    return iter(path)


def astar(start, end):
    global origin, goal
    origin = start
    goal = end
    frontier = []
    heapq.heappush(frontier, Node(0 + h(origin), 0, origin, None))
    explored = set()
    node = None
    while True:
        if len(frontier) > 0:
            node = heapq.heappop(frontier)
            if node.coord == goal:
                break
            explored.add(node.coord)
            for succ in succ8(node):
                if succ.coord not in explored and not contains(frontier, succ):
                    heapq.heappush(frontier, succ)
                else:
                    replace_if_exists(succ, frontier)
        else:
            raise Exception('Fail')
    return to_path(node)


def main():
    global origin, goal
    #origin = (673, 278)
    #goal = (630, 393)
    origin = (461, 963)
    goal = (857, 51)
    for coord in astar(origin, goal):
        a = coord


if __name__ == '__main__':
    profile.run('main()')